#include <WProgram.h>
#include "XBee2.h"
#include "../Streaming/Streaming.h"

bool _inApiMode = false;
XBee _xbee = XBee();
uint16_t _recipient = 0;

void XBee2::setRecipient(uint16_t recipient) {
  _recipient = recipient;
}

void XBee2::enterApiMode(bool apiMode) {
  delay(COMMAND_MODE_TIMEOUT);
  Serial << "+++";
  delay(COMMAND_MODE_TIMEOUT);
  Serial << "ATAP" << (apiMode ? '2' : '0') << endl << "ATCN" << endl;
  _inApiMode = apiMode;
}

bool XBee2::inApiMode() {
  return _inApiMode;
}

void XBee2::apiModeWrite(uint8_t b) {
  uint8_t payload[] = { b };
  Tx16Request tx = Tx16Request(_recipient, DISABLE_ACK_OPTION, payload, 1, NO_RESPONSE_FRAME_ID);
  _xbee.send(tx);
}

void XBee2::write(uint8_t b) {
  if (_inApiMode) {
    apiModeWrite(b);
  } else {
    Serial.write(b);
  }
}

void receiveStuffInApiMode(void(*handleCommand)(char)) {
  while (true) {
    _xbee.readPacket();
    if (!_xbee.getResponse().isAvailable()) {
      return;
    }
    if (_xbee.getResponse().getApiId() == RX_16_RESPONSE) {
      Rx16Response rx16 = Rx16Response();
      _xbee.getResponse().getRx16Response(rx16);
      for (int i = 0; i < rx16.getDataLength(); i++) {
        handleCommand(rx16.getData(i));
      }
    }
  }
}

void XBee2::receiveStuff(void(*handleCommand)(char)) {
  if (_inApiMode) {
    receiveStuffInApiMode(handleCommand);
  } else {
    while (Serial.available() > 0) {
      handleCommand(Serial.read());
    }
  }
}

